#include "bfposition.h"
#include <stdio.h>
#include <math.h>

//Funcion para retornar el driver
Driver* 
BFPosition_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return((Driver*)(new BFPosition(cf, section)));
}

// A driver registration function, again declared outside of the class so
// that it can be invoked without object context.  In this function, we add
// the driver into the given driver table, indicating which interface the
// driver can support and how to create a driver instance.
void BFPosition_Register(DriverTable* table)
{
  table->AddDriver("BFPosition", BFPosition_Init);
}

BFPosition::BFPosition(ConfigFile* cf, int section)
    : Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, 
             PLAYER_POSITION2D_CODE)
{
	//colors from configfile
	baseColor = cf->ReadInt(section,"baseColor",-1);
	robotColor = cf->ReadInt(section,"robotColor",-1);
	
	//if no colors were specified return
	if(baseColor==-1 || robotColor==-1)
	{
		PLAYER_ERROR("this driver requires baseColor and RobotColor to be specified");
    		return;
  	}
	//areas from configfile
	minArea = cf->ReadInt(section,"minArea",DEFAULT_MIN_AREA);
	maxArea = cf->ReadInt(section,"maxArea",DEFAULT_MAX_AREA);
	
	if(maxArea<=minArea)
	{
		minArea = DEFAULT_MIN_AREA;
		maxArea = DEFAULT_MAX_AREA;
	
	}
	//distances from configfile
	minDist = cf->ReadInt(section,"minDist",DEFAULT_MIN_DIST);
	maxDist = cf->ReadInt(section,"maxDist",DEFAULT_MAX_DIST);
	
	if(maxDist<=minDist || minDist<0 || maxDist<0)
	{
		minDist = DEFAULT_MIN_DIST;
		maxDist = DEFAULT_MAX_DIST;
	
	}
	
	//proyection matrix from configfile
	proyEnabled=false;
	
	printf("Reading proyection matrix values\n");
	for(int i=0;i<9;i++)
	{
		
		transMatrix[i] = cf->ReadTupleFloat(section,"matrix",i,-1);
		printf("value %d: %f\n",i,transMatrix[i]);
		if(transMatrix[i]!=-1)
			proyEnabled=true;
	}
	
	//matrix of intrinsic parameters from configfile
	intrParamEn=false;
	
	printf("Reading matrix of intrinsic parameters:\n");
	for(int i=0;i<9;i++)
	{
		
		intrinsicParams[i] = cf->ReadTupleFloat(section,"intrinsic",i,-1);
		printf("value %d: %f\n",i,intrinsicParams[i]);
		if(intrinsicParams[i]!=-1)
			intrParamEn=true;
	}
	
	
	radialCorrEnabled = false;
	//values for lens correction
	radialDistParams[0] = cf->ReadTupleFloat(section,"radial",0,-1);
	radialDistParams[1] = cf->ReadTupleFloat(section,"radial",1,-1);
	
	if(radialDistParams[0]!=-1 && radialDistParams[1]!=-1 && intrParamEn )
	{
		radialCorrEnabled = true;
		printf("radial Correction enabled!\n");
	}
	
	//requirement of a blobfinder
	if(cf->ReadDeviceAddr(&blobfinderAddr,section, "requires", PLAYER_BLOBFINDER_CODE, -1,NULL)!=0)
	{
    		PLAYER_ERROR("this driver requires a blobfinder in the .cfg file");
    		return;
  	}
	
	
	printf("BFPosition: Using baseColor: %d, robotColor: %d\n",baseColor,robotColor);
	printf("BFPosition: Using minArea: %d, maxArea: %d\n",minArea,maxArea);
	printf("BFPosition: Using minDist: %d, maxDist: %d\n",minDist,maxDist);
	if(proyEnabled)
		printf("BFPosition: Using proyection with given matrix!\n");
	
	
	
}



// Set up the device.  Return 0 if things go well, and -1 otherwise.
int BFPosition::Setup()
{   
	puts("bfposition driver initialising");

	//get device and subscribe to blobfinder device
	if (!(this->blobfinderDev = deviceTable->GetDevice(this->blobfinderAddr)))
	{
	PLAYER_ERROR("unable to locate suitable blobfinder device!");
	return(-1);
	}
	if(0 != this->blobfinderDev->Subscribe(this->InQueue))
	{
	PLAYER_ERROR("unable to subscribe to blobfinder device");
	return(-1);
	}

	puts("BFPosition driver ready");

	// Start the device thread; spawns a new thread and executes
	// ExampleDriver::Main(), which contains the main loop for the driver.
	StartThread();

	return(0);
}


// Shutdown the device
int BFPosition::Shutdown()
{
  puts("Shutting BFPosition down");

  // Stop and join the driver thread
  StopThread();

  // Unsubscribe from the blobfinder
  this->blobfinderDev->Unsubscribe(this->InQueue);


  puts("BFPosition has been shutdown");

  return(0);
}

int BFPosition::ProcessMessage(MessageQueue* resp_queue, 
                                  player_msghdr * hdr,
                                  void * data)
{
 // Handle new data from the blobfinder
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_BLOBFINDER_DATA_BLOBS,
                           this->blobfinderAddr))
  {
  	//get blobsdata
  	blobs_data = reinterpret_cast<player_blobfinder_data_t *>(data);
  	
  	//printf("Number of Blobs: %d\n",blobs_data->blobs_count);
  	
  	
  	//organize blobsdata in buffers
  	int BCIndex=0;
  	int RCIndex=0;
  	
  	for(uint i=0;i<blobs_data->blobs_count;i++)
  	{
  		player_blobfinder_blob_t blob = blobs_data->blobs[i]; 
  		
  		//blob has base color and its area is between min and max
  		if(blob.color == baseColor && BCIndex<MAX_BLOBS && blob.area>minArea && blob.area<maxArea)
  		{
  			BCBlobID[BCIndex++] = i;
  			//printf("base Color: %d\n",i);
  			//TODO: falta hacer que haga un break cuando encuentra la primera que no corresponde al minArea, pues están organizadas de mayor a menor
  			//le daria mas rapidez al sistema
  		}
  		
  		//blob has robot color and its area is between min and max
  		if(blob.color == robotColor && RCIndex<MAX_BLOBS && blob.area>minArea && blob.area<maxArea)
  		{
  			RCBlobID[RCIndex++] = i;
  			//printf("robot Color: %d\n",i);
  		}
  			
  	}
  	
  	//find couple of blobs
  	findBlobsCouple(BCIndex,RCIndex);
  	return(0);
  }
  
  
  
  //No other kind of messages will be handled. Return -1, so a message of type PLAYER_MSGTYPE_RESP_NACK will be sended back.
  return(-1);
  
}

void BFPosition::findBlobsCouple(int BColor, int RColor)
{
	int lastDist=0;
	int baseIndex=-1;
	int robotIndex=-1;
  
  	//calculate distances between each couple of blobs
  	
  	//TODO: CAMBIAR PARA QUE LA PRIMERA PAREJA QUE CUMPLA CON LOS REQUISITOS DE DISTANCIA SEA DEVUELTA, Y NO TENER QUE BUSCAR EN TODAS LAS PAREJAS
  	for(int b=0; b<BColor;b++)
  	{
	  	//recover blob of base color
	  	player_blobfinder_blob_t blobBase = blobs_data->blobs[BCBlobID[b]];
	  	for(int r=0;r<RColor;r++)
	  	{
	  		//recover blob of robot color
	  		player_blobfinder_blob_t blobRobot = blobs_data->blobs[RCBlobID[r]];
	  		//centroid coordinates between the two blobs
	  		int x = blobBase.x - blobRobot.x;
	  		int y = blobBase.y - blobRobot.y;
	  		
	  		int dist = x*x +y*y;
	  		
	  		//min and max distances were defined
	  		if(minDist<maxDist && minDist>0)
	  		{
		  		if(dist>=minDist*minDist && dist<=maxDist*maxDist)
			   	{
			   		baseIndex = b;
			   		robotIndex = r;
			   		break;
			   	}
	  		}
	  		else
	  		{
		  		//save first distance in the variable to be compared with the others
		  		
			   	if (!r && !b)
			  	{
			   		lastDist = dist;
			   		baseIndex = b;
			   		robotIndex = r;
			   	
			   	}
			   	// the other distances will be compared with the shortest one.
			   	else if(dist<lastDist){
			   		lastDist = dist;
			   		baseIndex = b;
			   		robotIndex = r;	
			   	}
		   	}
  		
  		}
	}
	
	if(baseIndex !=-1 && robotIndex!=-1)
	{
		//now we have the couple of blobs with minimal distance
		player_blobfinder_blob_t blobBase = blobs_data->blobs[BCBlobID[baseIndex]];
	  	player_blobfinder_blob_t blobRobot = blobs_data->blobs[RCBlobID[robotIndex]];
	  	
	  	//get center positions of blobs
	  	double y1 = blobBase.y;
	  	double x1 = blobBase.x;
	  	
	  	double y2 = blobRobot.y;
	  	double x2 = blobRobot.x;
	  	
	  	
	  	//if radial parameters were given, correct lens distortion
	  	if(radialCorrEnabled)
	  	{
	  		//copy of values
	  		double xx1 = x1;
	  		double yy1 = y1;
	  		
	  		double xx2 = x2;
	  		double yy2 = y2;
	  		
	  		correctLensDist(xx1,yy1,&x1,&y1);
	  		correctLensDist(xx2,yy2,&x2,&y2);
	  		
	  	}
	  	//if proyection has to be made, do it
	  	if(proyEnabled)
	  	{
	  		//copy of values
	  		int xx1 = (int)x1;
	  		int yy1 = (int)y1;
	  		
	  		int xx2 = (int)x2;
	  		int yy2 = (int)y2;
	  		
	  		TransformPoint(xx1,yy1,&x1,&y1);
	  		TransformPoint(xx2,yy2,&x2,&y2);
	  		
	  	}
	  	
	  	//calculate centroid and orientation
	  	double x = ((x1 + x2) / 2);
	  	double y = ((y1 + y2) / 2);
	  	
	  	double orientation = atan2((y1 - y2) , (x1 - x2))+ PI / 4;
	  	
	  	if(orientation>PI)
	  		orientation-=2*PI;
		player_pose_t pos;
	  	//player_pose_t vel;
	  	
	  	pos.px = x;
	  	pos.py = y;
	  	pos.pa = orientation;
	  	
	  	//printf("Pose: x: %f y: %f theta: %f\n",x,y,orientation);
	  	
	  	//save data in pos_data. If no valid couple was detected, use last position found.
	  	pos_data.pos = pos;
	  	
	  	
  	}
  	
  	Publish(device_addr,NULL,PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_STATE,reinterpret_cast<void*>(&pos_data),sizeof(player_position2d_data_t),NULL);
}

bool BFPosition::correctLensDist(double u, double v,double *X, double *Y)
{

		
	//get center of image
	double u0 = intrinsicParams[2];
	double v0 = intrinsicParams[5];
	//get f/dx and f/dy
	double fx = intrinsicParams[0];
	double fy = intrinsicParams[4];
	
	
	double x = (u-u0);
	double y = (v-v0);
	
	//now we have the coordinates related to the lens
	x/=fx;
	y/=fy;
	
	double r = x * x + y * y;  

	double radialX = x/(1 + radialDistParams[0]*r + radialDistParams[1]*r*r);
	
	*X = (radialX*fx + u0);
	
	double radialY = y/(1 + radialDistParams[0]*r + radialDistParams[1]*r*r);
	
	*Y = (radialY*fy + v0);

	return true;
	
}

void BFPosition::TransformPoint(int x, int y, double *X, double *Y)
{
	double imgPoint[]={x,y,1};//Punto dado
	double realPoint[3];

	realPoint[0] = transMatrix[0]*imgPoint[0] + transMatrix[1]*imgPoint[1] + transMatrix[2]*imgPoint[2];
	realPoint[1] = transMatrix[3]*imgPoint[0] + transMatrix[4]*imgPoint[1] + transMatrix[5]*imgPoint[2];
	realPoint[2] = transMatrix[6]*imgPoint[0] + transMatrix[7]*imgPoint[1] + imgPoint[2];
	
	
	
	*X = realPoint[0]/realPoint[2];
	*Y = realPoint[1]/realPoint[2];
}
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void BFPosition::Main() 
{
  // The main loop; interact with the device here
  for(;;)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Process incoming messages. In this  case, process blobfinder messages, and respond with
    // data of position. No other message will be treated, no request nor command.
    ProcessMessages();

    // Sleep (you might, for example, block on a read() instead)
    usleep(100000);
  }
}

////////////////////////////////////////////////////////////////////////////////
// Extra stuff for building a shared object.

/* need the extern to avoid C++ name-mangling  */
extern "C" {
  int player_driver_init(DriverTable* table)
  {
    puts("bfposition driver initializing");
    BFPosition_Register(table);
    puts("bfposition driver done");
    return(0);
  }
}
